#include "kalman_filter.h"


/**
  * @brief  卡尔曼滤波更新
  * @note   核心是融合测量值和系统模型预测值，消除随机噪声，
            即用动态加权平均，逐步逼近真实状态
  * @param  k_filter: 对象
  * @retval 0
  */
int32_t k_filter_update_f32(k_filter_f32_t *k_filter)
{
  /* 预测输入值 */
  k_filter->x_pred = k_filter->F * k_filter->x;
  
  /* 预测协方差更新 */
  k_filter->P_pred = k_filter->F * k_filter->P * k_filter->F + k_filter->Q;

  /* 计算卡尔曼增益 */
  k_filter->K = k_filter->P_pred / (k_filter->P_pred + k_filter->R);
  
  /* 融合预测与测量 */
  k_filter->x = k_filter->x_pred + k_filter->K * (k_filter->in - k_filter->x_pred);
  
  /* 更新协方差 */
  k_filter->P = (1 - k_filter->K) * k_filter->P_pred;
  
  /* 滤波后的输出 */
  k_filter->out = k_filter->x;
  
  return 0;
}




